#include "board_uart.h"
#include "board_config.h"
#include "platform.h"
#include "ring_buffer.h"
#include "sbus.h"
#include <stdio.h>
#include <errno.h>
#include <sys/unistd.h>

#define TX_BUF_LEN 64

UART_HandleTypeDef uart1handler;
UART_HandleTypeDef uart2handler;

ring_buffer uart1_tx_rb;
uint8_t u1_txbuf[TX_BUF_LEN];
uint8_t u1_dma_buf[TX_BUF_LEN];

#ifdef SBUS_ON_UART2
uint8_t u2_rx_dma_buf[25] = {0};
uint16_t rc_channels[18]={0};
#endif

void board_uart_init(UART_HandleTypeDef *uarthandler, uint32_t baudrate)
{
	if (uarthandler == &uart1handler) {
		uart1handler.Instance          = BOARD_UART1;

		uart1handler.Init.BaudRate     = baudrate;
		uart1handler.Init.WordLength   = UART_WORDLENGTH_8B;
		uart1handler.Init.StopBits     = UART_STOPBITS_1;
		uart1handler.Init.Parity       = UART_PARITY_NONE;
		uart1handler.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
		uart1handler.Init.Mode         = UART_MODE_TX_RX;
		uart1handler.Init.OverSampling = UART_OVERSAMPLING_16;

		HAL_UART_Init(&uart1handler);

		rb_init(&uart1_tx_rb, TX_BUF_LEN, u1_txbuf); 
		setbuf(stdout, NULL);
	} else if (uarthandler == &uart2handler) {
		uart2handler.Instance          = BOARD_UART2;

		uart2handler.Init.BaudRate     = baudrate;
		uart2handler.Init.WordLength   = UART_WORDLENGTH_8B;
		uart2handler.Init.StopBits     = UART_STOPBITS_1;
		uart2handler.Init.Parity       = UART_PARITY_NONE;
		uart2handler.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
		uart2handler.Init.Mode         = UART_MODE_TX_RX;
		uart2handler.Init.OverSampling = UART_OVERSAMPLING_16;

		HAL_UART_Init(&uart2handler);
#ifdef SBUS_ON_UART2
		HAL_UART_Receive_DMA(&uart2handler, u2_rx_dma_buf, 25);
#endif
	}
}

void HAL_UART_MspInit(UART_HandleTypeDef *huart)
{
	static DMA_HandleTypeDef hdma_tx;
	static DMA_HandleTypeDef hdma_rx;

	GPIO_InitTypeDef  GPIO_InitStruct;

	if (huart == &uart1handler) {
		/*##-1- Enable peripherals and GPIO Clocks #################################*/
		/* Enable GPIO TX/RX clock */
		BOARD_UART1_TX_GPIO_CLK_ENABLE();
		BOARD_UART1_RX_GPIO_CLK_ENABLE();
		/* Enable USART2 clock */
		BOARD_UART1_CLK_ENABLE();
		/* Enable DMA1 clock */
		BOARD_UART1_DMA_CLK_ENABLE();

		/*##-2- Configure peripheral GPIO ##########################################*/
		/* UART TX GPIO pin configuration  */
		GPIO_InitStruct.Pin       = BOARD_UART1_TX_PIN;
		GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
		GPIO_InitStruct.Pull      = GPIO_NOPULL;
		GPIO_InitStruct.Speed     = GPIO_SPEED_FAST;
		GPIO_InitStruct.Alternate = BOARD_UART1_TX_AF;

		HAL_GPIO_Init(BOARD_UART1_TX_GPIO_PORT, &GPIO_InitStruct);

		/* UART RX GPIO pin configuration  */
		GPIO_InitStruct.Pin = BOARD_UART1_RX_PIN;
		GPIO_InitStruct.Alternate = BOARD_UART1_RX_AF;

		HAL_GPIO_Init(BOARD_UART1_RX_GPIO_PORT, &GPIO_InitStruct);

		/*##-3- Configure the DMA streams ##########################################*/
		/* Configure the DMA handler for Transmission process */
		hdma_tx.Instance                 = BOARD_UART1_TX_DMA_STREAM;

		hdma_tx.Init.Channel             = BOARD_UART1_TX_DMA_CHANNEL;
		hdma_tx.Init.Direction           = DMA_MEMORY_TO_PERIPH;
		hdma_tx.Init.PeriphInc           = DMA_PINC_DISABLE;
		hdma_tx.Init.MemInc              = DMA_MINC_ENABLE;
		hdma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
		hdma_tx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
		hdma_tx.Init.Mode                = DMA_NORMAL;
		hdma_tx.Init.Priority            = DMA_PRIORITY_LOW;
		hdma_tx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
		hdma_tx.Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
		hdma_tx.Init.MemBurst            = DMA_MBURST_INC4;
		hdma_tx.Init.PeriphBurst         = DMA_PBURST_INC4;

		HAL_DMA_Init(&hdma_tx);

		/* Associate the initialized DMA handle to the the UART handle */
		__HAL_LINKDMA(huart, hdmatx, hdma_tx);

		/*##-4- Configure the NVIC for DMA #########################################*/
		/* NVIC configuration for DMA transfer complete interrupt (USARTx_TX) */
		HAL_NVIC_SetPriority(BOARD_UART1_DMA_TX_IRQn, 2, 1);
		HAL_NVIC_EnableIRQ(BOARD_UART1_DMA_TX_IRQn);

		/* NVIC configuration for USART TC interrupt */
		HAL_NVIC_SetPriority(BOARD_UART1_IRQn, 2, 0);
		HAL_NVIC_EnableIRQ(BOARD_UART1_IRQn);
	} else if (huart == &uart2handler) {
		/*##-1- Enable peripherals and GPIO Clocks #################################*/
		/* Enable GPIO TX/RX clock */
		BOARD_UART2_TX_GPIO_CLK_ENABLE();
		BOARD_UART2_RX_GPIO_CLK_ENABLE();
		/* Enable USART1 clock */
		BOARD_UART2_CLK_ENABLE();
		/* Enable DMA2 clock */
		BOARD_UART2_DMA_CLK_ENABLE();

		/*##-2- Configure peripheral GPIO ##########################################*/
		/* UART TX GPIO pin configuration  */
		GPIO_InitStruct.Pin       = BOARD_UART2_TX_PIN;
		GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
		GPIO_InitStruct.Pull      = GPIO_NOPULL;
		GPIO_InitStruct.Speed     = GPIO_SPEED_FAST;
		GPIO_InitStruct.Alternate = BOARD_UART2_TX_AF;

		HAL_GPIO_Init(BOARD_UART2_TX_GPIO_PORT, &GPIO_InitStruct);

		/* UART RX GPIO pin configuration  */
		GPIO_InitStruct.Pin = BOARD_UART2_RX_PIN;
		GPIO_InitStruct.Alternate = BOARD_UART2_RX_AF;

		HAL_GPIO_Init(BOARD_UART2_RX_GPIO_PORT, &GPIO_InitStruct);

		/*##-3- Configure the DMA streams ##########################################*/
		/* Configure the DMA handler for Transmission process */
		hdma_tx.Instance                 = BOARD_UART2_TX_DMA_STREAM;

		hdma_tx.Init.Channel             = BOARD_UART2_TX_DMA_CHANNEL;
		hdma_tx.Init.Direction           = DMA_MEMORY_TO_PERIPH;
		hdma_tx.Init.PeriphInc           = DMA_PINC_DISABLE;
		hdma_tx.Init.MemInc              = DMA_MINC_ENABLE;
		hdma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
		hdma_tx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
		hdma_tx.Init.Mode                = DMA_NORMAL;
		hdma_tx.Init.Priority            = DMA_PRIORITY_LOW;
		hdma_tx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
		hdma_tx.Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
		hdma_tx.Init.MemBurst            = DMA_MBURST_INC4;
		hdma_tx.Init.PeriphBurst         = DMA_PBURST_INC4;

		HAL_DMA_Init(&hdma_tx);

		/* Associate the initialized DMA handle to the the UART handle */
		__HAL_LINKDMA(huart, hdmatx, hdma_tx);

		/* Configure the DMA handler for Receiving process */
		hdma_rx.Instance                 = BOARD_UART2_RX_DMA_STREAM;

		hdma_rx.Init.Channel             = BOARD_UART2_RX_DMA_CHANNEL;
		hdma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
		hdma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
		hdma_rx.Init.MemInc              = DMA_MINC_ENABLE;
		hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
		hdma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
		hdma_rx.Init.Mode                = DMA_NORMAL;
		hdma_rx.Init.Priority            = DMA_PRIORITY_HIGH;
		hdma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
		hdma_rx.Init.FIFOThreshold       = DMA_FIFO_THRESHOLD_FULL;
		hdma_rx.Init.MemBurst            = DMA_MBURST_INC4;
		hdma_rx.Init.PeriphBurst         = DMA_PBURST_INC4;

		HAL_DMA_Init(&hdma_rx);
		__HAL_LINKDMA(huart, hdmarx, hdma_rx);

		HAL_NVIC_SetPriority(BOARD_UART2_DMA_RX_IRQn, 1, 1);
		HAL_NVIC_EnableIRQ(BOARD_UART2_DMA_RX_IRQn);

		HAL_NVIC_SetPriority(BOARD_UART2_DMA_TX_IRQn, 1, 2);
		HAL_NVIC_EnableIRQ(BOARD_UART2_DMA_TX_IRQn);

		/* NVIC configuration for USART TC interrupt */
		HAL_NVIC_SetPriority(BOARD_UART2_IRQn, 2, 0);
		HAL_NVIC_EnableIRQ(BOARD_UART2_IRQn);
	}
}

void HAL_UART_MspDeInit(UART_HandleTypeDef *huart)
{
	static DMA_HandleTypeDef hdma_tx;
	static DMA_HandleTypeDef hdma_rx;

	__HAL_LINKDMA(huart, hdmatx, hdma_tx);
	__HAL_LINKDMA(huart, hdmarx, hdma_rx);

	if (huart == &uart1handler) {
		/*##-1- Reset peripherals ##################################################*/
		BOARD_UART1_FORCE_RESET();
		BOARD_UART1_RELEASE_RESET();

		/*##-2- Disable peripherals and GPIO Clocks #################################*/
		/* Configure UART Tx as alternate function  */
		HAL_GPIO_DeInit(BOARD_UART1_TX_GPIO_PORT, BOARD_UART1_TX_PIN);
		/* Configure UART Rx as alternate function  */
		HAL_GPIO_DeInit(BOARD_UART1_RX_GPIO_PORT, BOARD_UART1_RX_PIN);

		/*##-3- Disable the DMA Streams ############################################*/
		/* De-Initialize the DMA Stream associate to transmission process */
		HAL_DMA_DeInit(&hdma_tx);
		/* De-Initialize the DMA Stream associate to reception process */
		HAL_DMA_DeInit(&hdma_rx);

		/*##-4- Disable the NVIC for DMA ###########################################*/
		HAL_NVIC_DisableIRQ(BOARD_UART1_DMA_TX_IRQn);
		HAL_NVIC_DisableIRQ(BOARD_UART1_DMA_RX_IRQn);
	} else if (huart == &uart2handler) {
		/*##-1- Reset peripherals ##################################################*/
		BOARD_UART2_FORCE_RESET();
		BOARD_UART2_RELEASE_RESET();

		/*##-2- Disable peripherals and GPIO Clocks #################################*/
		/* Configure UART Tx as alternate function  */
		HAL_GPIO_DeInit(BOARD_UART2_TX_GPIO_PORT, BOARD_UART2_TX_PIN);
		/* Configure UART Rx as alternate function  */
		HAL_GPIO_DeInit(BOARD_UART2_RX_GPIO_PORT, BOARD_UART2_RX_PIN);

		/*##-3- Disable the DMA Streams ############################################*/
		/* De-Initialize the DMA Stream associate to transmission process */
		HAL_DMA_DeInit(&hdma_tx);
		/* De-Initialize the DMA Stream associate to reception process */
		HAL_DMA_DeInit(&hdma_rx);

		/*##-4- Disable the NVIC for DMA ###########################################*/
		HAL_NVIC_DisableIRQ(BOARD_UART2_DMA_TX_IRQn);
		HAL_NVIC_DisableIRQ(BOARD_UART2_DMA_RX_IRQn);
	}
}

uint16_t board_uart_send(uint8_t dev_num, uint8_t* tx_buf, uint16_t size)
{
	if (dev_num == 0) {
		if (0 != HAL_UART_Transmit_DMA(&uart1handler, tx_buf, size)) {
			return 0;
		}
	} else if (dev_num == 1) {
		if (0 != HAL_UART_Transmit_DMA(&uart2handler, tx_buf, size)) {
			return 0;
		}
	}
	return size;
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef* uarthandler)
{
	if (uarthandler == &uart1handler) {
		uint16_t num = 0;
		while(!rb_is_empty(&uart1_tx_rb)) {
			u1_dma_buf[num++] = rb_remove(&uart1_tx_rb);
			if (num == TX_BUF_LEN) {
				break;
			}
		}
		if (num > 0) {
			HAL_UART_Transmit_DMA(&uart1handler, u1_dma_buf, num);
		} 
	} else if (uarthandler == &uart2handler) {
	}
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef* uarthandler)
{
	if (uarthandler == &uart1handler) {
	} else if (uarthandler == &uart2handler) {
#ifdef SBUS_ON_UART2
		uint8_t i = 0;
		for (; i < 25; i++) {
			if (SBUS_FRAME_OK == sbus_parse_char(u2_rx_dma_buf[i], rc_channels)) {
				break;
			}
		}
		HAL_UART_Receive_DMA(&uart2handler, u2_rx_dma_buf, 25);
#endif
	}
}

void board_stdout_flush(void)
{
#if BOARD_STDOUT_UART == 1
	uint32_t tmp1 = uart1handler.State;  
	if ((tmp1 == HAL_UART_STATE_READY) || (tmp1 == HAL_UART_STATE_BUSY_RX)) {
		HAL_UART_TxCpltCallback(&uart1handler);
	}
#endif
}

/* overwrite 
 * http://stm32f4-discovery.com/2014/10/use-printf-output-stream-stm32f4/
 * */

/*
 * read
 * Read a character to a file. `libc' subroutines will use this system routine for input from all files, including stdin
 * Returns -1 on error or blocks until the number of characters have been read.
 * */
int _read(int file, char *ptr, int len) 
{
//	int n;
//	int num = 0;
//	switch (file) {
//		case STDIN_FILENO:
//			for (n = 0; n < len; n++) {
//#if   BOARD_STDIN_UART == 1
//				while ((USART1->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {}
//				char c = (char)(USART1->DR & (uint16_t)0x01FF);
//#elif BOARD_STDIN_UART == 2
//				while ((USART2->SR & USART_FLAG_RXNE) == (uint16_t) RESET) {}
//				char c = (char) (USART2->DR & (uint16_t) 0x01FF);
//#elif BOARD_STDIN_UART == 3
//				while ((USART3->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {}
//				char c = (char)(USART3->DR & (uint16_t)0x01FF);
//#endif
//				*ptr++ = c;
//				num++;
//			}
//			break;
//		default:
//			errno = EBADF;
//			return -1;
//	}
//	return num;
	return 0;
}

/*
 * write
 * Write a character to a file. `libc' subroutines will use this system routine for output to all files, including stdout
 * Returns -1 on error or number of bytes sent
 * */
int _write(int file, char *ptr, int len) 
{
	int n;
	switch (file) {
		case STDOUT_FILENO: /*stdout*/
			for (n = 0; n < len; n++) {
#if BOARD_STDOUT_UART == 1
				if (!rb_insert(&uart1_tx_rb, ptr[n])) {
					break;
				}
#endif
			}
			break;
		case STDERR_FILENO: /* stderr */
			for (n = 0; n < len; n++) {
#if BOARD_STDERR_UART == 1
				if (!rb_insert(&uart1_tx_rb, ptr[n])) {
					break;
				}
#endif
			}
			break;
		default:
			errno = EBADF;
			return -1;
	}
	return n;
}
